
import time
from cmath import atan, nan, sqrt
from itertools import count
from re import X

import cv2
import numpy as np
import serial


def findLine(img0):
    # img=cv2.imread("runpy.jpg")
    # img0=cv2.imread("runpy.jpg")
    # img0=cv2.imread(frameSeries[photoCnt])
    gray=cv2.cvtColor(img0,cv2.COLOR_BGR2GRAY)
    blur=cv2.GaussianBlur(gray,(5,5),0)
    blur=blur.astype("uint8")
    ret,th=cv2.threshold(blur,0,255,cv2.THRESH_BINARY+cv2.THRESH_OTSU)
    th=cv2.dilate(th,None,iterations=2)
    th=cv2.erode(th,None,iterations=6)
    edges=cv2.Canny(th,70,80,apertureSize = 3)
    #cv2.imshow('otsu',edges)

    #Hough 
    #lines=cv2.HoughLinesP(edges,0.01,np.pi/180.0,10,minLineLength=30,maxLineGap=50)
    #for x1,y1,x2,y2 in lines[0]:
    #    cv2.line(img,(x1,y1),(x2,y2),(0,255,0),2)

    #LSD
    #lsd=cv2.createLineSegmentDetector(0)
    #dlines=lsd.detect(edges)
    #for dline in dlines[0]:
    #    x0=int(round(dline[0][0]))
    #    y0=int(round(dline[0][1]))
    #    x1=int(round(dline[0][2]))
    #    y1=int(round(dline[0][3]))
    #    cv2.line(img,(x0,y0),(x1,y1),(0,255,0),1,cv2.LINE_AA)

    #FLD
    fld=cv2.ximgproc.createFastLineDetector()
    dlines=fld.detect(edges)
    #drawn_img=fld.drawSegments(img,dlines,)
    for dline in dlines:
        x0=int(round(dline[0][0]))
        y0=int(round(dline[0][1]))
        x1=int(round(dline[0][2]))
        y1=int(round(dline[0][3]))
        cv2.line(img0,(x0,y0),(x1,y1),(0,255,0),4,cv2.LINE_AA)
    #1944  2592
    #cv2.imshow('lines',img)

    grayl=cv2.cvtColor(img0,cv2.COLOR_BGR2GRAY)
    blurl=cv2.GaussianBlur(grayl,(5,5),0)
    blurl=blurl.astype("uint8")
    _,thl=cv2.threshold(blurl,0,255,cv2.THRESH_BINARY+cv2.THRESH_OTSU)
    thl=cv2.dilate(thl,None,iterations=2)
    thl=cv2.erode(thl,None,iterations=6)
    # cv2.imshow('ostu',thl)
    edgesl=cv2.Canny(thl,70,80,apertureSize = 3)
    #cv2.imshow('img',edgesl)


    # HoughLineP Algorithm
    # linesl = cv2.HoughLinesP(edgesl, 3, np.pi / 180, 50, minLineLength=50, maxLineGap=100)
    # for line in linesl:
    #     x1, y1, x2, y2 = line[0]
    #     # x1 = line[0][0]
    #     # y1 = line[0][1]
    #     # x2 = line[0][2]
    #     # y2 = line[0][3]
    #     cv2.line(img, (x1, y1), (x2, y2), (255, 255, 0), 2)
    # cv2.imshow('lines',img)

    #HoughLine Algorithm
    # linesl = cv2.HoughLines(edgesl, 1, np.pi / 180, 100)
    # for line in linesl:
    #     rho = line[0][0]
    #     theta = line[0][1]
    #     a = np.cos(theta)
    #     b = np.sin(theta)
    #     x0 = a * rho
    #     y0 = b * rho
    #     x1 = int(x0 + 1000 * (-b))
    #     y1 = int(y0 + 1000 * (a))
    #     x2 = int(x0 - 1000 * (-b))
    #     y2 = int(y0 - 1000 * (a))
    #     cv2.line(img, (x1, y1), (x2, y2), (0, 0, 255), 2)
    # cv2.imshow('img1', img)


    #左右寻线法
    #Search the line on the right side
    #search from the middle of the img, the range is 2/3 of the height
    # img_array=np.array(edgesl)
    # shape=img_array.shape
    # print(shape)
    # height=shape[0]
    # width=shape[1]
    # pixW_right=[]
    # midPixH_right=[]
    # wMid=round(width/2)
    # searchHeight=round(1/3*height)
    # for midPixH in range(height-1,searchHeight,-1):
    #     for pixW in range(wMid,width-1,1):
    #         (b,g,r)=img_array[midPixH,pixW]
    #         if (b,g,r)==(0,255,0):
    #             #print(pixW,midPixH)
    #             pixW_left.append(pixW)
    #             midPixH_left.append(midPixH)
    #             break

    #Search the middle pixel of the white part
    img_array=np.array(thl)
    shape=img_array.shape
    bottomMid=int(shape[1]/2)
    searchHeightThr=int(shape[0]/8)
    trackLeftSeed=[]
    flagLeftSeed=0
    trackRightSeed=[]
    flagRightSeed=0
    for searchHeight in range(shape[0]-1,searchHeightThr,-1):
        for trackLeft in range(bottomMid,0,-1):
            gray=img_array[searchHeight][trackLeft]
            if gray==0:
                trackLeftSeed=(searchHeight,trackLeft)
                flagLeftSeed=1
                break
        if flagLeftSeed==1:
            break


    for searchHeight0 in range(shape[0]-1,searchHeightThr,-1):        
        for trackRight in range(bottomMid,shape[1]-1,1):
            gray=img_array[searchHeight0][trackRight]
            if gray==0:
                trackRightSeed=(searchHeight0,trackRight)
                flagRightSeed=1
                break
        if flagRightSeed==1:
            break    
        

    print(trackLeftSeed)
    print(trackRightSeed)

            
    #左侧赛道 Ltrack
    Ltrack=[]
    xvalueL=trackLeftSeed[1]
    grayscaleL=nan
    for i in range(trackLeftSeed[0],searchHeightThr,-1):
        for j in range(xvalueL,shape[1],1):
        #横坐标从本行向上寻找一行，再向右寻边
            grayscaleL=img_array[i][j]
            if grayscaleL==255:
                xvalueL=j-1
                Ltrack.append((i,j-1))
                break

    print(Ltrack[-1])

    # 右侧赛道 RTrack
    Rtrack=[]
    xvalueR=trackRightSeed[1]
    grayscaleR=nan
    for k in range(trackRightSeed[0],searchHeightThr,-1):
        for l in range(xvalueR,0,-1):
            # 横坐标从本行向上寻找一行，再向左寻边
            grayscaleR=img_array[k][l]
            if grayscaleR==255:
                xvalueR=l+1
                Rtrack.append((k,l+1))
                break

            
    print(Rtrack[-1])
    # 寻找左右赛道相对应的点，绘制中线
    # 寻找横坐标相等的左右对应点
    # 先比较对应点的纵坐标左右哪一个高，从较高的一边开始对应
    flagRight=0
    if trackRightSeed[0]>=trackLeftSeed[0]: #  右边y大于左边，右边较低，与左侧对齐
        pass
    else:
        flagRight=1

    # 将较低的list没有对应的地方跳过，到两边横坐标相等
    # 本段代码只用首尾两点找中线，适合直线
    index=0
    if flagRight==0:   
        index=len(Rtrack)-len(Ltrack)
        midPixBottomx=int(((Rtrack[index])[1]-(Ltrack[0])[1])/2+(Ltrack[0])[1])
        midPixBottomy=Ltrack[0][0]
        midPixTopx=int(((Rtrack[-1])[1]-(Ltrack[-1])[1])/2+(Ltrack[-1])[1]) 
    else:
        index=len(Ltrack)-len(Rtrack)
        midPixBottomx=int(((Rtrack[0])[1]-(Ltrack[index])[1])/2+(Ltrack[index])[1])
        midPixBottomy=Rtrack[0][0]
        midPixTopx=int(((Rtrack[-1])[1]-(Ltrack[-1])[1])/2+(Ltrack[-1])[1])
    

    midPixBottom=(midPixBottomx,midPixBottomy)
    midPixTop=(midPixTopx,searchHeightThr)
    print(midPixBottom)
    print(midPixTop)
    line=(midPixBottom,midPixTop)
    print(line)
    # cv2.line(thl,midPixBottom,midPixTop,0,3)
    # cv2.imshow('line',thl)
    return line

# 计算直线斜率
def slope(line):
    k=(line[0][1]-line[1][1])/(line[1][0]-line[0][0])
    return k

# 计算两条线中点间距，作为距离度量，单位是pixel
def distance(line1,line2):
    midPoint1=(int((line1[0][0]+line1[1][0])/2),int((line1[0][1]+line2[1][1])/2))
    midPoint2=(int((line2[0][0]+line2[1][0])/2),int((line2[0][1]+line2[1][1])/2))
    dis=sqrt(pow((midPoint1[0]-midPoint2[0]),2)+pow((midPoint1[1]-midPoint2[1]),2))
    return dis

# 计算两条线之间的夹角,返回值为弧度
def angle(line1,line2):
    slope1=slope(line1[0],line1[1])
    slope2=slope(line2[0],line2[1])
    agl=atan((slope2-slope1)/(1+slope1*slope2))
    return agl

# 弯道入弯检测，置标志位flagBend，与上位机通信



# main
# initialTime=time.time()
initialPhoto=cv2.imread("initial.jpg")
initialLine=findLine(initialPhoto)
frameSeries=[initialPhoto]
lineSeries=[initialLine]
distanceSeries=[0]
angleSeries=[0]
cam=cv2.VideoCapture(1)
photoCnt=1
#compare the last frame with the former
while(True):
    # time.sleep(1)
    # runtime=int(time.time()-initialTime)
    # print(runtime)
     #record the nth photo
    # timeCnt=runtime%5
    timeCnt=ser.inWaiting()# timeCnt 是接收上位机拍照标志位
    if timeCnt:
        print(photoCnt)
        _,frame=cam.read()
        photoCnt=photoCnt+1
        frameSeries.append(frame)
    # 处理当前帧
    lineFrame=findLine()
    lineSeries.append(lineFrame)
    distanceFrame=distance(lineFrame[photoCnt],lineFrame[photoCnt-1])
    errDistance=distanceSeries.append(distanceFrame)
    angleFrame=angle(lineFrame[photoCnt],lineFrame[photoCnt-1])
    ser=serial.Serial("/dev/ttyAMA0",115200)
    ser.write(distanceFrame)    # 发送距离数据
    ser.flushInput()
    ser.write(angleFrame)   # 发送角度数据
    ser.flushInput()
    
